#!/usr/bin/python2.5

from bluetooth import BluetoothError
from nxt import bluesock
from nxt import locator

class RobotError(Exception):
  """Base robot error exception class."""

class RobotNotConnected(RobotError):
  """Robot does not have a connection to an NXT brick."""


class BasicRobot(object):
  def __init__(self, name='robot', mac_address=None):
    self.name = name
    self.mac_address = mac_address
    self._brick = None
    self.sock = None
    self._default_power = 80

  def Run(self):
    raise NotImplemented()

  def Connect(self):

    if self.sock:
      self.sock.close()

    if self.mac_address:
      print "bluetooth"
      self.sock = bluesock.BlueSock(self.mac_address)
    else:
      try:
        print "locating"
        self.sock = locator.find_one_brick()
      except locator.BrickNotFound, e:
        raise RobotNotConnected('Error locating brick: %^s' % e)

    print "connecting"
    self._brick = self.sock.connect()

  def ShutDown(self):
    self.sock.close()

  def SetPower(self, power_level):
    if power_level > 127:
      self._default_power = 127
    elif power_level < -128:
      self._default_power = -128
    else:
      self._default_power = power_level

  def Brick(self):
    if self._brick:
      try:
        dev_info = self._brick.get_device_info()
        return self._brick
      except Exception:
        pass
    raise RobotNotConnected('NXT brick not connected')

def ConnectToBrick(mac_address):
  sock = nxt.bluesock.BlueSock(mac_address)
  brick = sock.connect()
  return brick


def PrintBrickInfo(brick):
  name, host, signal_strength, user_flash = brick.get_device_info()
  print 'NXT brick name: %s' % name
  print 'Host address: %s' % host
  print 'Bluetooth signal strength: %s' % signal_strength
  print 'Free user flash: %s' % user_flash
  millivolts = brick.get_battery_level()
  print 'Battery level %s mV' % millivolts
